#include <iostream>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <unistd.h>
#include <cstring>
#include <cerrno>
#include <net/if.h>
#include <vector>
#include <cmath>
#include "cansend.h"
// 接收CAN帧的函数
void ReceiveData(int sock)
{
    struct can_frame frame;
    struct sockaddr_can addr;
    socklen_t len = sizeof(addr);

    while (true)
    {
        // 接收CAN帧
        int nbytes = recvfrom(sock, &frame, sizeof(struct can_frame), 0, (struct sockaddr *)&addr, &len);
        if (nbytes < 0)
        {
            std::cerr << "接收CAN帧失败！" << std::endl;
            continue;
        }

        // 根据CAN ID进行分类处理
        switch (frame.can_id)
        {
        case 0x123:
        {
            std::cout << "RX ID:0x" << std::hex << frame.can_id << std::endl;
            for (int i = 0; i < frame.can_dlc; i++)
            {
                std::cout << " data:" << std::hex << (int)frame.data[i];
            }
            std::cout << std::endl;

            // 处理数据
            uint8_t receiveData[8];
            for (int i = 0; i < frame.can_dlc; i++)
            {
                receiveData[i] = frame.data[i];
            }

            uint8_t changeData[3];
            uint32_t hexNumber = 0;
            int32_t encoder2_data;

            changeData[0] = receiveData[2];
            changeData[1] = receiveData[1];
            changeData[2] = receiveData[0];

            // 将data中的三个字节组合成一个32位的十六进制数
            hexNumber = (changeData[0] << 16) | (changeData[1] << 8) | changeData[2];

            std::cout << "hexNumber: 0x" << std::hex << hexNumber << std::endl;

            // 检查符号位
            if (hexNumber & 0x800000)
            {
                // 如果符号位为1，表示负数
                hexNumber |= 0xFF000000; // 扩展符号位到32位
                encoder2_data = (int32_t)hexNumber;
            }
            else
            {
                encoder2_data = (int32_t)hexNumber;
            }

            float angle_joint1 = ((float)encoder2_data) * 360.0 / 16384.0;
            int joint_1data = round(angle_joint1);

            std::cout << "角度是：" << joint_1data << std::endl;

            // joint_1data发布
            break;
        }

        default:
        {
            std::cout << "收到未处理的CAN ID: 0x" << std::hex << frame.can_id << std::endl;
            break;
        }
        }
    }
}

int main()
{
    int can0 = init_can("can0");
    // 开始接收CAN数据
    ReceiveData(can0);
    // 关闭CAN套接字
    close(can0);

    return 0;
}
